import pytz
import datetime
import numpy as np


class DecodeOption:
    def __init__(self):
        pass


class GnssData:
    def __init__(self):
        self.time_stamp = 0
        self.week = 0
        self.gpst = 0
        self.lat = 0
        self.lon = 0
        self.height = 0
        self.pos_std = [0, 0, 0]
        self.ns = 0
        self.mode = 0

    def reset(self):
        self.time_stamp = 0
        self.week = 0
        self.gpst = 0
        self.lat = 0
        self.lon = 0
        self.height = 0
        self.pos_std = [0, 0, 0]
        self.ns = 0
        self.mode = 0

    def is_valid(self):
        if self.time_stamp <= 0: return False
        if self.week <= 0: return False
        if self.lat <= 0: return False
        if self.lon <= 0: return False
        if self.height <= 0: return False
        if self.pos_std[0] <= 0: return False
        if self.pos_std[1] <= 0: return False
        if self.pos_std[2] <= 0: return False
        if self.ns <= 0: return False
        if self.mode <= 0: return False
        # if sel <= 0 : return False
        return True
        # self.lat = 0


def ddmmmm2deg(indata: str, d_length: int):
    """
    ddmm.mmmmm转换为deg
    :param indata:
    :param d_length: 2:dd or 3:ddd
    :return:
    """
    dd = int(indata[0:d_length])
    mm = float(indata) - 100 * dd
    return dd + mm / 60


class NmeaDecoder:
    def __init__(self, opt: DecodeOption):
        self.opt = opt
        self.gnss = GnssData()
        self.out = None
        self.unduation  = 9.2255

    def __parse_gga(self, data: list):
        try:
            self.gnss.lat = ddmmmm2deg(data[2], 2)
            self.gnss.lon = ddmmmm2deg(data[4], 3)
            self.gnss.time_stamp = int(float(data[1]))
            self.gnss.mode = int(data[6])
            # q = float(data[6])
            self.gnss.ns = float(data[7])
            self.gnss.height = float(data[9])
            self.unduation = float(data[11])

        except ValueError as e:
            print(data)
            print(self.gnss.time_stamp, e)
            self.unduation = -9.2255
        except IndexError as e:
            print(data)
            print(self.gnss.time_stamp, e)
            self.unduation = -9.2255
        self.gnss.height += self.unduation  # 海拔转换为大地高
        # hdop = float(data[8])
        # self.undulation = float(data[11])
        pass

    def __parse_rmc(self, strrmc: list):
        try:
            if self.gnss.time_stamp != int(float(strrmc[1])): return
            year = int(strrmc[9][4:6]) + 2000
            mon = int(strrmc[9][2:4])
            dd = int(strrmc[9][0:2])
            hh = int(strrmc[1][0:2])
            mm = int(strrmc[1][2:4])
            ss = int(strrmc[1][4:6])
            ms = int(strrmc[1][7:-1])
            utc_tz = pytz.timezone('UTC')
            cur_time = datetime.datetime(year, mon, dd, hh, mm, ss, ms, tzinfo=utc_tz) + datetime.timedelta(0, 18, 0, 0,
                                                                                                            0)  # GPST - UTC0 = 18s
            gps_0_time = datetime.datetime(1980, 1, 6, 0, 0, 0, 0, tzinfo=utc_tz)
            diff_time = cur_time - gps_0_time

            self.gnss.week = np.floor(diff_time.days / 7)
            self.gnss.gpst = 86400 * (diff_time.days % 7) + diff_time.seconds + diff_time.microseconds / 1000
        except ValueError as e:
            print(strrmc)
            print(self.gnss.time_stamp, e)
        except IndexError as e:
            print(strrmc)
            print(self.gnss.time_stamp, e)
        pass

    def __parse_gst(self, strgst: list):
        try:
            if self.gnss.time_stamp != int(float(strgst[1])): return
            self.gnss.pos_std[0] = float(strgst[6])
            self.gnss.pos_std[1] = float(strgst[7])
            self.gnss.pos_std[2] = float(strgst[8].split('*')[0])
        except ValueError as e:
            print(strgst)
            print("GST", self.gnss.time_stamp, e)
        except IndexError as e:
            print(strgst)
            print("GST", self.gnss.time_stamp, e)
        pass

    def __save_gnss(self):
        string = ("{:d} {:.5f} {:.12f} {:.12f} {:.5f} {:.3f} {:.3f} {:.3f} {:d} {:d}\n".format(
            int(self.gnss.week),
            int(self.gnss.gpst),
            self.gnss.lat,
            self.gnss.lon,
            self.gnss.height,
            self.gnss.pos_std[0],
            self.gnss.pos_std[1],
            self.gnss.pos_std[2],
            int(self.gnss.ns),
            int(self.gnss.mode),
        ))
        self.out.write(string)

    def parse_log(self, log_file_name: str):
        self.out = open(log_file_name + ".gnsstxt", 'w')
        with open(log_file_name, 'r') as f:
            while True:
                line_list = f.readline().split(',')
                # print(line_list)
                if line_list[0] == "": break;
                if len(line_list) < 1: continue
                if line_list[0] == "$GPGGA": self.__parse_gga(line_list)
                if line_list[0] == "$GNGGA": self.__parse_gga(line_list)
                if line_list[0] == "$GNRMC": self.__parse_rmc(line_list)
                if line_list[0] == "$GPRMC": self.__parse_rmc(line_list)
                if line_list[0] == "$GPGST": self.__parse_gst(line_list)
                if line_list[0] == "$GNGST": self.__parse_gst(line_list)
                if self.gnss.is_valid():
                    self.__save_gnss()
                    self.gnss.reset()


def test_nmea():
    logfilename = "/media/rebeater/hd_data2/workspace/raw_data/2022/20200302/Reference/cutecom20220302_ub482_2.log"
    opt = DecodeOption()
    decoder = NmeaDecoder(opt)
    decoder.parse_log(logfilename)


if __name__ == '__main__':
    test_nmea()
